#ifndef EOPDALIGN
#define EOPDALIGN
#endif

#define LOW_PASS_EOPD_ALPHA 0.35

/*
* MUST HAVE KAMLIN-SMUX RUNNING FROM BEGINNING OF MATCH
* Spinning right (clockwise) is greater
*@bool alignSide: aligns to the dispensor, returns false if it couldn't complete
*  - bool side: true, red side, false, blue side
*  - bool leftRight: true if right, false is left of the dispensor
*@procedure alignDist: aligns from a certain length from the wall.
*  - int dist: the distance from the wall in EOPD counts
*@procedure leftRight: Alligns with the dispencer left and right
*  - bool side: true is right, false in left
*/
bool alignSide(bool sideRed, bool leftSide);
void alignDist(int dist);
void leftRight(bool sideRead);
void _alignSpinCompass(int compassEnd);
void _alignSpinEOPD();
int lowPassEOPD(bool sideRead);
void StartEOPD();
int _lft = 0;
int _rgt = 0;
int compassEnd = 0;

bool alignSide(bool sideRed, bool leftSide) {
  int readingR;
  int readingL;
  readingR = lowPassEOPD(RIGHTSD);
  readingL = lowPassEOPD(LEFTSD);
  if (readingR == -1 || readingL == -1) {   //if something isn't connected correctly, then return -1
      return false;
  }
  if (sideRed) {
    compassEnd = 0;
  }
  else if (!sideRed) {
    compassEnd = 180;
  }
  _alignSpinCompass(compassEnd);
  _alignSpinEOPD();
  alignDist(DISTANCE_TO_WALL);
  _alignSpinEOPD();
  alignDist(DISTANCE_TO_WALL + 4);
  writeDebugStreamLine("DONE w/spin");
  leftRight(leftSide);
  return true;
}

void _alignSpinEOPD() {
  int readingL;
  int readingR;
  readingR = lowPassEOPD(RIGHTSD) * 1.8;
  readingL = lowPassEOPD(LEFTSD);
  while (!_isBetween(readingL, readingR, 2)) {
    readingR = lowPassEOPD(RIGHTSD);
    readingL = lowPassEOPD(LEFTSD);
    writeDebugStreamLine("Done Reading");
    if (readingR > readingL + 1) {
      clk(9);
      writeDebugStreamLine("        Turning Clock");
    }
    else if (readingL > readingR + 1) {
      cntClk(9);
      writeDebugStreamLine("        Turning Counter Clock");
    }
  }
  stopmot();
}


void _alignSpinCompass(int compassEnd) {
  while (!_isBetween(heading, compassEnd, 1)){
    int headingNeg;
    if (heading > 180) {
      headingNeg = heading - 360;
    }
    else {
      headingNeg = heading;
    }
    if (headingNeg > compassEnd) {
      cntClk(14);
    }
    else if (headingNeg < compassEnd) {
      clk(14);
    }
  }
  stopmot();
}

void alignDist(int dist) {
  bool done = false;
  int _readingR;
  int _readingL;
  while (!done) {
    _readingR = lowPassEOPD(RIGHTSD);
    _readingL = lowPassEOPD(LEFTSD);
    if (_readingL < dist || _readingR < dist) {
      forward(SPEED_MOVEMENTS);
    }
    else if (_readingL > dist || _readingR > dist) {
      backward(SPEED_MOVEMENTS);
    }
    if (_isBetween(_readingR, dist, 1) || _isBetween(_readingL, dist, 1))  {
      stopmot();
      done = true;
    }
  }
  stopmot();
}

void leftRight(bool sideRead) {
  int _readingR;
  int _readingL;
  _readingR = lowPassEOPD(RIGHTSD);
  _readingL = lowPassEOPD(LEFTSD);
  if (sideRead == RIGHTSD) {
    left(15);
    while (_readingL < DISTANCE_OF_DISPENSOR) {
      _readingL = lowPassEOPD(LEFTSD);
    }
    stopmot();
    wait1Msec(5000);
    PlaySound(soundException);
    left(15);
    while (abs(nMotorEncoder[b]) < 970) {
      writeDebugStream("" +nMotorEncoder[b] + "\n");
    }
    writeDebugStreamLine("Motor: %4d", nMotorEncoder[b]);
    stopmot();
  }
  else if (sideRead == LEFTSD) {
    right(15);
    while (_readingR < DISTANCE_OF_DISPENSOR) {
      _readingR = lowPassEOPD(RIGHTSD);
    }
    stopmot();
    right(15);
    while (abs(nMotorEncoder[b]) < 970) {
      nxtDisplayTextLine(1, "%d",nMotorEncoder[b]);
    }
    stopmot();
  }
}


int lowPassEOPD(bool sideRead) {
  if (sideRead == LEFTSD) {
    _lft = _lft * (1-LOW_PASS_EOPD_ALPHA) + (LOW_PASS_EOPD_ALPHA * EOPDLReading());
    writeDebugStreamLine("READING: "+_lft);
    return _lft;
  }
  else {
    _rgt = _rgt * (1-LOW_PASS_EOPD_ALPHA) + (LOW_PASS_EOPD_ALPHA * EOPDRReading());
    return _rgt;
  }
}

void StartEOPD() {
  _lft = EOPDLReading();
  _rgt = EOPDRReading();
  for (int i = 0; i < 10; i++) {
    lowPassEOPD(RIGHTSD);
    lowPassEOPD(LEFTSD);
  }
}
